
#include  "signal_convert.h"

STR_LPF_HDL is_dcbus_lpf = {0.2f,0};

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void mcu_signal_convert(void)
{
	
	// get ADC convert result 
	userHdl.signal.rslt.vdc                = ADC2_Rslt_DMA[2];
	userHdl.signal.rslt.is_wheel[0]        = ADC0_Rslt_DMA[0];
	userHdl.signal.rslt.is_wheel[1]        = ADC0_Rslt_DMA[1];
	userHdl.signal.rslt.is_mainbrush       = ADC0_Rslt_DMA[2];
	userHdl.signal.rslt.is_dustsuctionfan  = ADC0_Rslt_DMA[3];
	userHdl.signal.rslt.is_leftbrush       = ADC2_Rslt_DMA[1];

	// Vdc voltage sample convert 
	userHdl.signal.real.vdc = userHdl.signal.rslt.vdc * VOLTAGE_CONVERT_SF;
	
	// motor current convert
	userHdl.signal.real.is_wheel[0]       = userHdl.signal.rslt.is_wheel[0] * CURRENT_CONVERT_SF;
	userHdl.signal.real.is_wheel[1]       = userHdl.signal.rslt.is_wheel[1] * CURRENT_CONVERT_SF;
	userHdl.signal.real.is_mainbrush      = userHdl.signal.rslt.is_mainbrush * CURRENT_CONVERT_SF;
	userHdl.signal.real.is_dustsuctionfan = userHdl.signal.rslt.is_dustsuctionfan * CURRENT_CONVERT_SF;
	userHdl.signal.real.is_leftbrush      = userHdl.signal.rslt.is_leftbrush * CURRENT_CONVERT_SF;
	
	userHdl.signal.battery_level = userHdl.signal.real.vdc * BATTERY_RATE_COFF_K + BATTERY_RATE_COFF_B;
	userHdl.signal.battery_level = MATH_sat(userHdl.signal.battery_level, 100, 0);
	
	userHdl.signal.real.is_dcbus = 
			(DCBUS_CURRENT_SF * userHdl.signal.rslt.is_dcbus - 2.5f) / 0.4f + DCBUS_CURRENT_SENSOR_ZERO;
			
	userHdl.signal.real.is_dcbus = signal_low_pass(&is_dcbus_lpf, userHdl.signal.real.is_dcbus);
	
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void wall_cliff_signal_convert(void)
{
	uint16_t ad_rslt_temp = 0;
	
	// get sensor ADC result
	userHdl.signal.rslt.wall_sensor[0] = ADC2_Rslt_DMA[0];
	userHdl.signal.rslt.wall_sensor[1] = ADC0_Rslt_DMA[4];
	
	userHdl.signal.rslt.cliff_sensor[0] = ADC2_Rslt_DMA[3];
	userHdl.signal.rslt.cliff_sensor[1] = ADC2_Rslt_DMA[4];
	userHdl.signal.rslt.cliff_sensor[2] = ADC0_Rslt_DMA[6];
	userHdl.signal.rslt.cliff_sensor[3] = ADC0_Rslt_DMA[7];

	// calcuate sensor real value
	ad_rslt_temp = MATH_sat(userHdl.signal.rslt.wall_sensor[0], WALL_SENSOR_AD_MAX, WALL_SENSOR_AD_MIN);
	userHdl.signal.real.wall_sensor[0] = linerInterLookUp(ad_rslt_temp, &WALL_SENSOR_Table[0][0]);
	
	ad_rslt_temp = MATH_sat(userHdl.signal.rslt.wall_sensor[1], WALL_SENSOR_AD_MAX, WALL_SENSOR_AD_MIN);
	userHdl.signal.real.wall_sensor[1] = linerInterLookUp(ad_rslt_temp, &WALL_SENSOR_Table[0][0]);

	ad_rslt_temp = MATH_sat(userHdl.signal.rslt.cliff_sensor[0], CLIFF_SENSOR_AD_MAX, CLIFF_SENSOR_AD_MIN);
	userHdl.signal.real.cliff_sensor[0] = linerInterLookUp(ad_rslt_temp, &WALL_SENSOR_Table[0][0]);

	ad_rslt_temp = MATH_sat(userHdl.signal.rslt.cliff_sensor[1], CLIFF_SENSOR_AD_MAX, CLIFF_SENSOR_AD_MIN);
	userHdl.signal.real.cliff_sensor[1] = linerInterLookUp(ad_rslt_temp, &WALL_SENSOR_Table[0][0]);

	ad_rslt_temp = MATH_sat(userHdl.signal.rslt.cliff_sensor[2], CLIFF_SENSOR_AD_MAX, CLIFF_SENSOR_AD_MIN);
	userHdl.signal.real.cliff_sensor[2] = linerInterLookUp(ad_rslt_temp, &WALL_SENSOR_Table[0][0]);

	ad_rslt_temp = MATH_sat(userHdl.signal.rslt.cliff_sensor[3], CLIFF_SENSOR_AD_MAX, CLIFF_SENSOR_AD_MIN);
	userHdl.signal.real.cliff_sensor[3] = linerInterLookUp(ad_rslt_temp, &WALL_SENSOR_Table[0][0]);
	
	
	if((userHdl.ctrl.shield_cliff == 0) && (userHdl.ctrl.shield_cliff_A == 0))
	{
		for(uint8_t i=0;i<4;i++)
		{
			if(userHdl.signal.real.cliff_sensor[i] > CLIFF_DIAG_THRE_MM)
			{
				userHdl.signal.cliff_flg[i] = 1;	
			}
			
			else if(userHdl.signal.real.cliff_sensor[i] < (CLIFF_DIAG_THRE_MM - CLIFF_DIAG_SCHMIDT))
			{
				userHdl.signal.cliff_flg[i] = 0;	
			}
		}
		
		// meidi backleft & backright cliff sensor
		
		if(userHdl.signal.real.wall_sensor[0] > CLIFF_DIAG_THRE_MM)
		{
			userHdl.signal.cliff_flg[4] = 1;	
		}
		
		else if(userHdl.signal.real.wall_sensor[0] < (CLIFF_DIAG_THRE_MM - CLIFF_DIAG_SCHMIDT))
		{
			userHdl.signal.cliff_flg[4] = 0;	
		}
		
		if(userHdl.signal.real.wall_sensor[1] > CLIFF_DIAG_THRE_MM)
		{
			userHdl.signal.cliff_flg[5] = 1;	
		}
		
		else if(userHdl.signal.real.wall_sensor[1] < (CLIFF_DIAG_THRE_MM - CLIFF_DIAG_SCHMIDT))
		{
			userHdl.signal.cliff_flg[5] = 0;	
		}
		
	}
	else
	{
		userHdl.signal.cliff_flg[0] = 0;
		userHdl.signal.cliff_flg[1] = 0;
		userHdl.signal.cliff_flg[2] = 0;
		userHdl.signal.cliff_flg[3] = 0;
		userHdl.signal.cliff_flg[4] = 0;
		userHdl.signal.cliff_flg[5] = 0;
	}

	userHdl.signal.cliff_status = 
		((userHdl.signal.cliff_flg[0] << 0) + (userHdl.signal.cliff_flg[1] << 1) +  \
		 (userHdl.signal.cliff_flg[2] << 2) + (userHdl.signal.cliff_flg[3] << 3) +  \
		 (userHdl.signal.cliff_flg[4] << 4) + (userHdl.signal.cliff_flg[5] << 5));
	
	
	if(userHdl.signal.cliff_status != userHdl.signal.cliff_status_old)
	{
		//upload_uartcomm_data(ID_CLIFF_UPLOAD);
	}
	
	userHdl.signal.cliff_status_old = userHdl.signal.cliff_status;
}

